#ifndef __MSG_CONVERT_H__
#define __MSG_CONVERT_H__

#include <geometry_msgs/Pose.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> 
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <iostream>
#include <vector>
using namespace std;
using namespace Eigen;

#include "base_define.h"

namespace msg_convert{
    // tf2::Quaternion -> Euler
    std::tuple<double, double, double> Quaternion2Euler(tf2::Quaternion quaternion);
    // geometry_msgs::Quaternion -> Euler
    std::tuple<double, double, double> GeometryQuaternion2Euler(geometry_msgs::Quaternion orientation);
    // Euler -> tf2::Quaternion
    tf2::Quaternion Euler2Quaternion(double roll, double pitch, double yaw);
    // Euler -> geometry_msgs::Quaternion
    geometry_msgs::Quaternion Euler2GeometryQuaternion(double roll, double pitch, double yaw);


    // tf2::Transform -> geometry_msgs::TransformStamped
    geometry_msgs::TransformStamped Transform2Geometry(tf2::Transform transform_tf);
    // geometry_msgs::TransformStamped -> tf2::Transform
    tf2::Transform Geometry2Transform(geometry_msgs::TransformStamped transform_stamped);
    // geometry_msgs::TransformStamped -> Eigen::Affine3f
    Eigen::Affine3f Geometry2Affine(geometry_msgs::TransformStamped transform);

    /***********************************************************
     * ToVec2f
     ***********************************************************/
    // Pose2D -> Vec2f
    Vec2f ToVec2f(Pose2D pose_2d);

    /***********************************************************
     * ToPose2D
     ***********************************************************/
    // Pose2D -> string
    string Pose2D2Str(Pose2D pose);
     // Vec2f -> Pose2D
    Pose2D ToPose2D(Vec2f vec_2f);
    // pcl::PointXYZ -> Pose2D
    Pose2D ToPose2D(pcl::PointXYZ point);
    // geometry_msgs::Pose -> Pose2D
    Pose2D ToPose2D(geometry_msgs::Pose pose);

    /***********************************************************
     * ToGeometryPose
     ***********************************************************/
    // Pose2D -> geometry_msgs::Pose
    geometry_msgs::Pose ToGeometryPose(Pose2D pose);
    // Vec2f -> geometry_msgs::Pose
    geometry_msgs::Pose ToGeometryPose(Vec2f pose);
    // geometry_msgs::Point -> geometry_msgs::Pose
    geometry_msgs::Pose ToGeometryPose(geometry_msgs::Point point);

    // Pose2D -> geometry_msgs::PoseStamp
    geometry_msgs::PoseStamped ToGeometryPoseStamp(Pose2D pose, string frame_id);
    // Vec2f -> geometry_msgs::PoseStamped
    geometry_msgs::PoseStamped ToGeometryPoseStamp(Vec2f pose, string frame_id);
    // geometry_msgs::Point -> geometry_msgs::PoseStamped
    geometry_msgs::PoseStamped ToGeometryPoseStamp(geometry_msgs::Point point, string frame_id);

    /***********************************************************
     * ToPath
     ***********************************************************/
    // vector<Pose2D> -> nav_msgs::Path
    nav_msgs::Path ToPath(vector<Pose2D> poses, string frame_id);
    // vector<Vec2f> -> nav_msgs::Path
    nav_msgs::Path ToPath(vector<Vec2f> poses, string frame_id);
    // vector<geometry_msgs::Point> -> nav_msgs::Path
    nav_msgs::Path ToPath(vector<geometry_msgs::Point> points, string frame_id);

    /***********************************************************
     * ToPointCloud2
     ***********************************************************/
    sensor_msgs::PointCloud2 ToPointCloud2(vector<geometry_msgs::Point> points, string frame_id);

    /***********************************************************
     * Velocity2D <-> geometry_msgs::Twist
     ***********************************************************/
    // Velocity2D -> string
    string Velocity2D2Str(Velocity2D vel);
    // Velocity2D -> geometry_msgs::Twist
    geometry_msgs::Twist ToGeometryTwist(Velocity2D velocity);
    // geometry_msgs::Twist -> Velocity2D
    Velocity2D ToVelocity2D(geometry_msgs::Twist twist);


    // 转换2D数据为点云数据 sensor_data::Scan2DPtr -> sensor_data::PointCloudXYZPtr
    sensor_data::PointCloudXYZPtr convertToPointCloudXYZ(
        sensor_data::Scan2DPtr scan_ptr);
    // 转换点云数据  sensor_data::PointCloud2Ptr -> sensor_data::PointCloudXYZPtr
    sensor_data::PointCloudXYZPtr convertToPointCloudXYZ(
        sensor_data::PointCloud2Ptr pointcloud2_ptr);
    // 转换到PointCloud2  sensor_data::PointCloudXYZPtr -> sensor_msgs::PointCloud2
    sensor_msgs::PointCloud2Ptr convertToPointCloud2(sensor_data::PointCloudXYZPtr pointcloud_ptr);
    // TF变换 pcl::PointCloud<pcl::PointXYZ>::Ptr  Eigen::Affine3f
    void transformPointXYZ(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out, const Eigen::Affine3f &transform);
    // sensor_msgs::PointCloud2::Ptr -> sensor_msgs::PointCloud
    sensor_msgs::PointCloud convertToPointCloud(sensor_msgs::PointCloud2Ptr pointcloud2_ptr);
        

} // namespace MapConvert

#endif